#include "sm2_fp.h"

const sm2_fp sm2_zero = {0};
const sm2_fp sm2_one = {1};
const sm2_fp sm2_p  = {0xFFFFFFFF, 0xFFFFFFFF, 0x00000000, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFE};
const sm2_fp sm2_n  = {0x39D54123, 0x53BBF409, 0x21C6052B, 0x7203DF6B, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFE};
const sm2_fp sm2_a  = {0xFFFFFFFC, 0xFFFFFFFF, 0x00000000, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFE};
const sm2_fp sm2_b  = {0x4D940E93, 0xDDBCBD41, 0x15AB8F92, 0xF39789F5, 0xCF6509A7, 0x4D5A9E4B, 0x9D9F5E34, 0x28E9FA9E};
const sm2_fp sm2_s  = {0x00000000, 0x80000000, 0x80000000, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF};
// const sm2_fp sm2_gx = {0x334C74C7, 0x715A4589, 0xF2660BE1, 0x8FE30BBF, 0x6A39C994, 0x5F990446, 0x1F198119, 0x32C4AE2C};
// const sm2_fp sm2_gy = {0x2139F0A0, 0x02DF32E5, 0xC62A4740, 0xD0A9877C, 0x6B692153, 0x59BDCEE3, 0xF4F6779C, 0xBC3736A2};

//**********************************************************************************************************************

inline void sm2_fp_set(sm2_fp r, const sm2_fp a) {
    memcpy(r, a, sizeof(sm2_fp));
}

inline int sm2_fp_is_odd(const sm2_fp a){
    return (int) a[0] & 1;
}

inline int sm2_fp_is_even(const sm2_fp a){
    return (int) (a[0] & 1) ^ 1;
}

inline int sm2_fp_is_zero(const sm2_fp a) {
    return a[0] == 0 && a[1] == 0 && a[2] == 0 && a[3] == 0 && a[4] == 0 && a[5] == 0 && a[6] == 0 && a[7] == 0;
}

inline int sm2_fp_non_zero(const sm2_fp a) {
    return a[0] > 0 || a[1] > 0 || a[2] > 0 || a[3] > 0 || a[4] > 0 || a[5] > 0 || a[6] > 0 || a[7] > 0;
}

inline int sm2_fp_equ(const sm2_fp a, const sm2_fp b) {
    return a[0] == b[0] && a[1] == b[1] && a[2] == b[2] && a[3] == b[3] && a[4] == b[4] && a[5] == b[5] && a[6] == b[6] && a[7] == b[7];
}

void sm2_fp_rand(sm2_fp r, const sm2_fp p) {
    sm2_fp_sub(r, p, sm2_one);
}



inline void sm2_fp_tri(sm2_fp r, const sm2_fp a){
    sm2_fp_dbl(r, a);
    sm2_fp_add(r, r, a);
}


inline void sm2_fp_cub(sm2_fp r, const sm2_fp a) {
    sm2_fp_sqr(r, a);
    sm2_fp_mul(r, r, a);
}

const char byte_to_hstr[] = {
    '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F',
};

const u8 hstr_to_byte[] = {
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 0, 0, 0, 0, 0, 0,
    0, 10, 11, 12, 13, 14, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
    0, 10, 11, 12, 13, 14, 15, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
};

void bytes_from_hexstr(void *b, const char* s, const u32 nbytes) {
    u8 *byte = b;
    for (int i = 0; i < nbytes; i ++)
        byte[i] = hstr_to_byte[(u8)s[i << 1]] << 4 | hstr_to_byte[(u8)s[(i << 1) + 1]];
}

void sm2_fp_from_str(sm2_fp n, const char* s) {
    u32 t[64];
    for (int i = 0; i < 64; i++)
        t[63 - i] = hstr_to_byte[(u8)s[i]];
    for (int i = 0; i < 8; i++) {
        n[i] = 0;
        for (int j = 0; j < 8; j++)
            n[i] |= t[(i << 3) + j] << (j << 2);
    }
}

void sm2_fp_to_str(const sm2_fp n, char* s) {
    u8 t[64];
    for (int i = 0; i < 8; i++)
        for (int j = 0; j < 8; j++)
            t[(i << 3) + j] = (n[i] >> (j >> 2)) & 0x0f;
    for (int i = 0; i < 64; ++i)
        s[63 - i] = byte_to_hstr[t[i]];
}

void sm2_bn_from_str(sm2_bn b, const char* s) {
    bytes_from_hexstr(b, s, 32);
}

void sm2_bn_to_str(const sm2_bn b, char* s) {
    const char lbyt_to_str[256] = {
        '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '0', '1', '1', '1', '1', '1', '1', '1', '1', '1', '1', '1', '1', '1', '1', '1', '1',
        '2', '2', '2', '2', '2', '2', '2', '2', '2', '2', '2', '2', '2', '2', '2', '2', '3', '3', '3', '3', '3', '3', '3', '3', '3', '3', '3', '3', '3', '3', '3', '3',
        '4', '4', '4', '4', '4', '4', '4', '4', '4', '4', '4', '4', '4', '4', '4', '4', '5', '5', '5', '5', '5', '5', '5', '5', '5', '5', '5', '5', '5', '5', '5', '5',
        '6', '6', '6', '6', '6', '6', '6', '6', '6', '6', '6', '6', '6', '6', '6', '6', '7', '7', '7', '7', '7', '7', '7', '7', '7', '7', '7', '7', '7', '7', '7', '7',
        '8', '8', '8', '8', '8', '8', '8', '8', '8', '8', '8', '8', '8', '8', '8', '8', '9', '9', '9', '9', '9', '9', '9', '9', '9', '9', '9', '9', '9', '9', '9', '9',
        'A', 'A', 'A', 'A', 'A', 'A', 'A', 'A', 'A', 'A', 'A', 'A', 'A', 'A', 'A', 'A', 'B', 'B', 'B', 'B', 'B', 'B', 'B', 'B', 'B', 'B', 'B', 'B', 'B', 'B', 'B', 'B',
        'C', 'C', 'C', 'C', 'C', 'C', 'C', 'C', 'C', 'C', 'C', 'C', 'C', 'C', 'C', 'C', 'D', 'D', 'D', 'D', 'D', 'D', 'D', 'D', 'D', 'D', 'D', 'D', 'D', 'D', 'D', 'D',
        'E', 'E', 'E', 'E', 'E', 'E', 'E', 'E', 'E', 'E', 'E', 'E', 'E', 'E', 'E', 'E', 'F', 'F', 'F', 'F', 'F', 'F', 'F', 'F', 'F', 'F', 'F', 'F', 'F', 'F', 'F', 'F'};
    const char rbyt_to_str[256] = {
        '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F',
        '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F',
        '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F',
        '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F',
        '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F',
        '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F',
        '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F',
        '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F', '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
    for (int i = 0, j = 0; i < 32; i++, j += 2) {
        s[j] = lbyt_to_str[b[i]];
        s[j + 1] = rbyt_to_str[b[i]];
    }
}

void sm2_fp_to_bn(const sm2_fp n, sm2_bn b) {
    const u8 *t = (const u8 *) n;
    for (int i = 0; i < 32; i++)
        b[i] = t[31 - i];
}

void sm2_bn_to_fp(const sm2_bn b, sm2_fp n) {
    u8 *t = (u8 *) n;
    for (int i = 0; i < 32; i++)
        t[31 - i] = b[i];
}

void sm2_fp_naf(naf r, const u8 w, const sm2_fp n) {
    if (w > 7)
        return;     // w > 7 is not supported unless the naf type definition is expanded from i8[257] to int[257]

    int i = 256;
    sm2_fp k, t;
    sm2_fp_set(k, n);
    while (sm2_fp_cmp(k, sm2_one)) {
        if (sm2_fp_is_odd(k)) {
            sm2_fp_set(t, sm2_zero);
            t[0] = k[0] & ((1 << w) - 1);       // t[0] = k[0] mod 2^w
            if (t[0] >> (w - 1)) {
                t[0] = (1 << w) - t[0];
                r[i] = (i8) -t[0];
            } else {
                r[i] = (i8) t[0];
            }
            if (r[i] > 0)
                sm2_fp_sub(k, k, t);
            else
                sm2_fp_add(k, k, t);
        } else
            r[i] = 0;
        sm2_fp_haf(k, k);
        i--;
    }
    while (i >= 0) {
        r[i--] = 0;
    }
}

void sm2_fp_nafp(i8 K[52], const sm2_fp k) {
    naf kn;
    sm2_fp_naf(kn, 2, k);
    int i, j = 0;
    for (i = 256; i > 2; i = i-5){
        K[j++] = (i8) (kn[i] + (kn[i-1]<<1) + (kn[i-2]<<2) + (kn[i-3]<<3) + (kn[i-4]<<4));
    }
    K[j] = (i8) (kn[1] + (kn[0]<<1));
}
